#include "Creature.h"


void Creature::addMass(const v2& _Pos,const float _Mass)
{
    _masses.push_back(EvoMass(_Pos,_Mass));
}

void Creature::addSpring(const int _M0,const int _M1,const float _Strength,const float _Dampening)
{
    const float length = (_masses[_M1]._pos-_masses[_M0]._pos).len();
    _springs.push_back(EvoSpring(_M0,_M1,length,_Strength,_Dampening));
}

void Creature::update()
{
    for(size_t i=0; i<_springs.size(); ++i)
    {
        EvoSpring& s = _springs[i];

        // *** muscle flexing ***
        s._extension = 1.f + sinf(SIMULATION_TIME*s._frequency+s._phase)*s._amplitude;
        
        // *** physics ***
        EvoMass& m0 = _masses[s._masses[0]];
        EvoMass& m1 = _masses[s._masses[1]];
        
        v2 separation = m1._pos-m0._pos;
        s._current_length = separation.normalise();
        const float force = (s._rest_length*s._extension - s._current_length)*s._strength;
        
        separation *= force;
        
        m0._pos -= separation;
        m1._pos += separation;
    }
    for(size_t i=0; i<_masses.size(); ++i)
    {
        EvoMass& m = _masses[i];
        m._pos[1] += GRAVITY;
        
        // *** verlet integration ***
        v2 vel = m._pos-m._old_pos;
        m._old_pos = m._pos;
        m._pos += vel*GLOBAL_MASS_DAMPING;
        
        // *** contrain points to screen ***
        if( m._pos[0]<0 ) m._pos[0] = 0;
        if( m._pos[0]>WINDOW_WIDTH ) m._pos[0] = WINDOW_WIDTH;
        if( m._pos[1]>WINDOW_HEIGHT ) m._pos[1] = WINDOW_HEIGHT;
    }
}

void Creature::draw()
{
    glLineWidth(1.f);
    glColor3f(0,1,0);
    glBegin(GL_LINES);
    for(size_t i=0; i<_springs.size(); ++i)
    {
        EvoSpring& s = _springs[i];
        glVertex2fv(_masses[s._masses[0]]._pos.ptr());
        glVertex2fv(_masses[s._masses[1]]._pos.ptr());
    }
    glEnd();

    glPointSize(3.f);
    glColor3f(0,1,0);
    glBegin(GL_POINTS);
    for(size_t i=0; i<_masses.size(); ++i)
    {
        glVertex2fv(_masses[i]._pos.ptr());
    }
    glEnd();
}